﻿using System;
//using System.Collections.Generic;
//using System.ComponentModel;
//using System.Data;
using System.Diagnostics;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using Microsoft.Ccr.Core;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Robotics.Services.IRobot.Lite.Proxy;
using Microsoft.Robotics.Services.Sensors.Kinect.Proxy;
using NestedParticleFilter;
using Point = System.Drawing.Point;

namespace iCreateSimulation
{
    public partial class ImageForm : Form
    {

        private readonly KinectOperations _iKinectPort;
        private readonly IRobotLiteOperations _iDrivePort;
        private readonly iCreateSimulationOperations _mainPort;
        private readonly KinectOperations _jKinectPort;
        private readonly IRobotLiteOperations _jDrivePort;
        delegate void SetPoseCallback(RobotPose pose);

        private delegate void SetPointCallback(NestedParticleFilter.Point point);
        private delegate void SetBitmap(Image img);

        public void SetBoydPicForI(Image image)
        {

            if (image == null)
                return;
            if (boydPicBoxForI.InvokeRequired)
            {
                var d = new SetBitmap(SetBoydPicForI);
                Invoke(d, new object[] { image });
            }
            else
            {
                boydPicBoxForI.Image = image;
            }

        }
        public void SetBoydPicFoJ(Image image)
        {

            if (image == null)
                return;
            if (boydPicBoxForJ.InvokeRequired)
            {
                var d = new SetBitmap(SetBoydPicFoJ);
                Invoke(d, new object[] { image });
            }
            else
            {
                boydPicBoxForJ.Image = image;
            }

        }
        public void SetIKinectPic(Image image)
        {

            if (image == null)
                return;
            if (iPicBox.InvokeRequired)
            {

                var d = new SetBitmap(SetIKinectPic);
                Invoke(d, new object[] { image });
            }
            else
            {
                iPicBox.Image = image;
            }
        }
        public void SetJKinectPic(Image image)
        {

            if (image == null)
                return;
            if (jPicBox.InvokeRequired)
            {

                var d = new SetBitmap(SetJKinectPic);
                Invoke(d, new object[] { image });
            }
            else
            {
                jPicBox.Image = image;
            }
        }
        public void SetIDepthPic(Image image)
        {
            if (image == null)
                return;
            
            if (iDepthPicBox.InvokeRequired)
            {

                var d = new SetBitmap(SetIDepthPic);
                Invoke(d, new object[] { image });
            }
            else
            {
                iDepthPicBox.Image = image;
            }

        }
        public void SetJDepthPic(Image image)
        {

            if (image == null)
                return;
            if (image == null)
                return;
            
            if (jDepthPicBox.InvokeRequired)
            {

                var d = new SetBitmap(SetJDepthPic);
                Invoke(d, new object[] { image });
            }
            else
            {
                jDepthPicBox.Image = image;
            }

        }

        public ImageForm(iCreateSimulationOperations mainPort,KinectOperations iKinectPort, IRobotLiteOperations iDrivePort, KinectOperations jKinectPort, IRobotLiteOperations jDrivePort)
        {
            _iKinectPort = iKinectPort;
            _iDrivePort = iDrivePort;
            _jKinectPort = jKinectPort;
            _jDrivePort = jDrivePort;
            _mainPort = mainPort;
            InitializeComponent();
        }
      

        /// <summary>
        /// Changing the pat's real pose text safely
        /// </summary>
        /// <param name="pose"></param>
        public void SetPatRealPose(RobotPose pose)
        {
            if (pose == null)
                return;
            
            if (this._txtPatPose.InvokeRequired)
            {
                SetPoseCallback d = new SetPoseCallback(SetPatRealPose);
                this.Invoke(d, new object[] { pose });

            }
            else
            {
                this._txtPatPose.Text = pose.X.ToString("##.##")
                    + "\t" + pose.Z.ToString("##.##") + "\t" + pose.Theta.ToString("##.##"); 

            }
        }
        public void SetPatClusteredPose (RobotPose pose)
        {
            if (pose == null)
                return;
            if (this._txtClusteredPose.InvokeRequired)
            {
                SetPoseCallback d = new SetPoseCallback(SetPatClusteredPose);
                this.Invoke(d, new object[] { pose });

            }
            else
            {
                this._txtClusteredPose.Text = pose.X.ToString("##.##")
                    + "\t" + pose.Z.ToString("##.##") + "\t" + pose.Theta.ToString("##.##"); 

            }
        }

        public void SetPatAveragePose(RobotPose pose)
        {
            if (pose == null)
                return;
            if (this._txtAveragedPose.InvokeRequired)
            {
                SetPoseCallback d = new SetPoseCallback(SetPatAveragePose);
                this.Invoke(d, new object[] { pose });

            }
            else
            {
                this._txtAveragedPose.Text = pose.X.ToString("##.##")
                    + "\t" + pose.Z.ToString("##.##") + "\t" + pose.Theta.ToString("##.##"); 

            }
        }

        public void SetWayPoint (NestedParticleFilter.Point point)
        {
            if (point == null)
                return;
            if (this._txtWayPoint.InvokeRequired)
            {
                SetPointCallback d = new SetPointCallback(SetWayPoint);
                this.Invoke(d, new object[] {point});
                
            }
            else
            {
                this._txtWayPoint.Text = point.ToString();
            }
        }

        private void btnStopOoboo_Click(object sender, EventArgs e)
        {
            _iDrivePort.CreateDriveDirect(0,0);
            _jDrivePort.CreateDriveDirect(0, 0);
            _mainPort.Post(new SimulationReset());
                
        }

        

        private void btnStartOoboo_Click(object sender, EventArgs e)
        {
            _iDrivePort.RoombaPlaySong(1);
            _mainPort.Post(new PatExplore());
            _mainPort.Post(new MatExplore());
            iCreateSimulationService._startTime = DateTime.UtcNow;

        }
      
        /// <summary>
        /// Button click handler for setting kinect angle of robot's i
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void iSetKinectBtn_Click(object sender, EventArgs e)
        {

            int angle = 0;
            bool success = int.TryParse(iAngleTxtBox.Text, out angle);

            if (success && angle <= 27 && angle >= -27)
            {
                UpdateTiltRequest tiltRequest = new UpdateTiltRequest();
                tiltRequest.Tilt = angle;
                _iKinectPort.UpdateTilt(tiltRequest);
            }
            else
            {
                UpdateTiltRequest tiltRequest = new UpdateTiltRequest();
                tiltRequest.Tilt = 0;
                _iKinectPort.UpdateTilt(tiltRequest);
            }

        }
      
        private void iSetVelocityBtm_Click(object sender, EventArgs e)
        {
            try
            {
                int speed;
                bool success = int.TryParse(iVelocityTxtBox.Text, out speed);
                if (success)
                    iCreateSimulationService.IForwardVelocity = speed;
            }
            catch(Exception exception)
            {
                Debug.WriteLine(exception.ToString());
            }
        }

        private void terminateBtm_Click(object sender, EventArgs e)
        {
            _iDrivePort.CreateDriveDirect(0, 0);
            
            _mainPort.Post(new DsspDefaultDrop());
        }

        private void jSetKinectBtn_Click(object sender, EventArgs e)
        {
            int angle = 0;
            bool success = int.TryParse(jAngleTxtBox.Text, out angle);

            if (success && angle <= 27 && angle >= -27)
            {
                UpdateTiltRequest tiltRequest = new UpdateTiltRequest();
                tiltRequest.Tilt = angle;
                _jKinectPort.UpdateTilt(tiltRequest);
            }
            else
            {
                UpdateTiltRequest tiltRequest = new UpdateTiltRequest();
                tiltRequest.Tilt = 0;
                _jKinectPort.UpdateTilt(tiltRequest);
            }
        }

        private void jSetVelocityBtn_Click(object sender, EventArgs e)
        {

            try
            {
                int speed;
                bool success = int.TryParse(jVelocityTxtBox.Text, out speed);
                if (success)
                    iCreateSimulationService.JForwardVelocity = speed;
            }
            catch (Exception exception)
            {
                Debug.WriteLine(exception.ToString());
            }
        }

      
      
       

       

       
       
    }
}
